classdef PIDOptimizer < handle
    %PIDOPTIMIZER Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        plant
        filter
        system
        controller
    end
    
    properties(Access=private)
        pRange
        iRange
        dRange
        kTc
        timeList
    end
    
    methods
        function obj = PIDOptimizer(plant, varargin)
            %PIDOPTIMIZER Construct an instance of this class
            %   Detailed explanation goes here
            p = inputParser;
            p.addRequired('plant', @(x) isa(x, 'EquipmentControl.FeedBackOptimization.LTIUnit.AbstractLTIUnit') || isa(x, 'tf'));
            p.addOptional('filter', EquipmentControl.FeedBackOptimization.LTIUnit.StaticGain(1.0), ...
                                     @(x) isa(x, 'EquipmentControl.FeedBackOptimization.LTIUnit.AbstractLTIUnit') || isa(x, 'tf'));
            p.parse(plant, varargin{:});
                                

            if isa(p.Results.plant, 'tf')
                num = p.Results.plant.Numerator{1}; den = p.Results.plant.Denominator{1};
                plant = EquipmentControl.FeedBackOptimization.LTIUnit.AbstractLTIUnit('plant', num, den);
            end
            
            obj.plant = plant;
            obj.filter = p.Results.filter;
            obj.system = obj.plant*obj.filter;
            obj.controller = obj.PIController_1stOrderApprox();
        end
        %% I/O
        function tl = getTimeList(obj)
            tl = obj.timeList;
        end
        function setRangeP(obj, pRange)
            obj.pRange = pRange;
        end
        function setRangeI(obj, iRange)
            obj.iRange = iRange;
        end
        function setRangeD(obj, dRange)
            obj.dRange = dRange;
        end
        function range = getRangeP(obj)
            range = obj.pRange;
        end
        function range = getRangeI(obj)
            range = obj.iRange;
        end
        function range = getRangeD(obj)
            range = obj.dRange;
        end
        function [kp, ki, kd] = getPIDValue(obj)
            [kp, ki, kd] = obj.controller.getPIDValues();
        end        
        %% Fisrt Order Approximation        
        function [c, Kp, Ki] = PIController_1stOrderApprox(obj)
            sys = obj.system;
            try
                [~, tau1, tDelay] = sys.firstOrderApprox();
            catch
                error('The plant does not have a 1st-order approximation.');
            end
            t_expect = sys.tc;
            Kp = tau1/sys.gain/(t_expect+tDelay);
            Ki = Kp/min(tau1, 4.0*(t_expect+tDelay));
            obj.setController(Kp, Ki, 0.0);
            c = obj.controller;
        end
        %% controller
        function obj = setController(obj, Kp, Ki, Kd)
            p=inputParser;
            p.addRequired('Kp', @isnumeric);
            p.addRequired('Ki', @isnumeric);
            p.addRequired('Kd', @isnumeric);
            p.parse(Kp, Ki, Kd);
            
            c = EquipmentControl.FeedBackOptimization.LTIUnit.PIDController(p.Results.Kp, ...
                                                                            p.Results.Ki, ...
                                                                            p.Results.Kd);
            obj.controller = c;
        end
        
        function obj = setController1stOrderApprox(obj)
            obj.controller = obj.PIController_1stOrderApprox();
        end
        function obj = setControllerOptimal(obj, tList, varargin)
            obj.controller = obj.optimPID('tList', tList, varargin{:});
        end
        
        function lti = referenceTracking(obj)
            G=obj.controller*obj.system; 
            refTrack_G = feedback(G.tfFunction,1);
            lti = EquipmentControl.FeedBackOptimization.LTIUnit.AbstractLTIUnit('RefTrack', refTrack_G.Numerator{1}, refTrack_G.Denominator{1});
        end
        
        function lti = noiseRejection(obj)
            noiseRej_G = feedback(obj.system.tfFunction, obj.controller.tfFunction);
            lti = EquipmentControl.FeedBackOptimization.LTIUnit.AbstractLTIUnit('NoiseReject', noiseRej_G.Numerator{1}, noiseRej_G.Denominator{1});
        end
        
        function lti = noiseRejection1(obj)
            G=obj.controller*obj.system;
            noiseRej_G = feedback(1, G.tfFunction);
            lti = EquipmentControl.FeedBackOptimization.LTIUnit.AbstractLTIUnit('NoiseReject1', noiseRej_G.Numerator{1}, noiseRej_G.Denominator{1});
        end
        %% Plot
        function [y, t] = closeLoopStepResponse(~, lti, varargin)
            p=inputParser;
            axCheck = @(x) isa(x, 'matlab.graphics.axis.Axes') || isempty(x);
            p.addParameter('tList', lti.tList, @isnumeric);
            p.addParameter('isPlot', true, @islogical);
            p.addParameter('axes', [], axCheck);
            p.parse(varargin{:});

            [y, t] = lti.stepResponse('tList', p.Results.tList, ...
                                      'isPlot', p.Results.isPlot, ...
                                      'axes', p.Results.axes, ...
                                      'calcApprox', false);
            
        end
        function [y, t] = referenceTrackingResponse(obj, varargin)
            lti = obj.referenceTracking();
            [y, t] = obj.closeLoopStepResponse(lti, varargin{:});
        end
        
        function [y, t] = noiseRejectionResponse(obj, varargin)
            lti = obj.noiseRejection();
            [y, t] = obj.closeLoopStepResponse(lti, varargin{:});            
        end
        
        function [y, t] = noiseRejection1Response(obj, varargin)
            lti = obj.noiseRejection1();
            [y, t] = obj.closeLoopStepResponse(lti, varargin{:});            
        end
        
        function [y1, y2, tList] = response(obj, tList, varargin)
            p=inputParser;
            p.addRequired('tList', @isnumeric);
            p.addParameter('isPlot', true, @islogical);
            p.parse(tList, varargin{:});
            
            lti1 = obj.referenceTracking();
            lti2 = obj.noiseRejection();
            
            y1 = lti1.stepResponse('isPlot', false, 'tList', p.Results.tList);
            y2 = lti2.stepResponse('isPlot', false, 'tList', p.Results.tList);
            
            if p.Results.isPlot
                ax1 = subplot(2, 1, 1);
                plot(ax1, p.Results.tList, y1, 'ro-')
                legend(ax1, {'Ref Track'}, 'location', 'best'); xlim(ax1, minmax(p.Results.tList));
                [pVal, iVal, dVal] = obj.getPIDValue();
                grid(ax1, 'on'); xlabel(ax1, 'Time (s)'); 
                title(ax1, sprintf('c=%s, P=%5.2e, I=%5.2e, D=%5.2e', obj.controller.name, pVal, iVal, dVal));
                
                ax2 = subplot(2, 1, 2);
                plot(ax2, p.Results.tList, y2, 'bd-');
                legend(ax2, {'Noise Reject'}, 'location', 'best'); xlim(ax2, minmax(p.Results.tList));
                grid(ax2, 'on'); xlabel(ax2, 'Time (s)'); 
            end
        end
        
        function bodePlot(obj, varargin)
            p=inputParser;
            axCheck = @(x) isa(x, 'matlab.graphics.axis.Axes');
            loopCheck = @(x) ismember(x, {'CloseLoop', 'OpenLoop'});
            p.addParameter('isPlot', true, @islogical);
            p.addParameter('axesMag', gca, axCheck);
            p.addParameter('axesPhase', gca, axCheck);
            p.addParameter('loop', 'CloseLoop', loopCheck);
            p.parse(varargin{:});
            
            switch p.Results.loop
                case 'CloseLoop'
                    lti = obj.referenceTracking();                    
                case 'OpenLoop'
                    lti = obj.system;
            end
            tffunc = lti.tfFunction;
            
            if isempty( varargin )
                argNames = {};
            else
                argNames = varargin{1:2:end};
            end
            
            if p.Results.isPlot
                if any(ismember({'axesMag', 'axesPhase'}, argNames))

                    [mag,phase,wout] = bode(tffunc, logspace(log10(lti.wMin), log10(lti.wMax), 100));
                    
                    w3dB = lti.bw_3dB;
                    
                    loglog(p.Results.axesMag, wout, mag(:), 'bo-'); hold(p.Results.axesMag, 'on');
                    yl = ylim(p.Results.axesMag); ylim(p.Results.axesMag, [max(yl(1), 1e-2) yl(2)]);                    
                    loglog(p.Results.axesMag, wout, sqrt(0.5)*ones(size(wout)), 'k--');
                    loglog(p.Results.axesMag, [w3dB, w3dB], ylim(p.Results.axesMag), 'k--');
                    
                    grid(p.Results.axesMag, 'on'); 
                    legend(p.Results.axesMag, sprintf('f_{3dB}=%3.2f Hz', w3dB), 'Location','best');
                    hold(p.Results.axesMag, 'off');
                    ylabel(p.Results.axesMag, 'Magnetude Gain');
                    
                    semilogx(p.Results.axesPhase, wout, phase(:), 'rd-'); hold(p.Results.axesPhase, 'on');                                         
                    yl = ylim(p.Results.axesPhase); ylim(p.Results.axesPhase, [max(yl(1), -360) yl(2)]);  
                    grid(p.Results.axesPhase, 'on'); 
                    if lti.isStable
                        [~,Pm,~,Wpm] = lti.margins();
                        semilogx(p.Results.axesPhase, [Wpm Wpm], ylim(p.Results.axesPhase), 'k--');
                        semilogx(p.Results.axesPhase, wout, -180+Pm*ones(size(wout)), 'k--');
                        legend(p.Results.axesPhase, sprintf('\\phi=%3.2f{\\circ}, \n f_p=%3.2f Hz', Pm, Wpm), 'Location','best');
                    end
                    xlabel(p.Results.axesPhase, 'Frequency (Hz)');
                    ylabel(p.Results.axesPhase, 'Phase (deg)');
                    hold(p.Results.axesPhase, 'off')
                else
                    bode(tffunc); grid on;
                end
            end
        end        
        %% Plot
        function [Kp1, Ki1, rangeP, rangeI] = getDefaultRange(obj)
            [~, Kp1, Ki1] = obj.PIController_1stOrderApprox();
            rangeP = minmax([0.0, 5*Kp1]);
            rangeI = minmax([0.0, 5*Ki1]);
        end
        function [X, Y, Z, minP, maxP, minI, maxI, Kp1, Ki1] = PIChart(obj, varargin)
            p=inputParser;
            [Kp1, Ki1, ~, ~] = obj.getDefaultRange();
            costCheck = @(x) ismember(x, {'ISE', 'ITSE', 'IAE', 'ITAE'});
            weightCheck = @(x) x>=0 && x<=1.0;
            axCheck = @(x) isa(x, 'matlab.graphics.axis.Axes');
            
            p.addParameter('minP', 0.0, @isnumeric);
            p.addParameter('maxP', 3.0*obj.controller.p, @isnumeric);
            p.addParameter('minI', 0.0, @isnumeric);
            p.addParameter('maxI', 3.0*obj.controller.i, @isnumeric);
            p.addParameter('dValue', obj.controller.d, @isnumeric);
            p.addParameter('delay', 0, @isnumeric);
            p.addParameter('costfun', 'ITAE', costCheck);
            p.addParameter('weight', 1.0, weightCheck);
            p.addParameter('isPlot', true, @islogical);
            p.addParameter('axes', gca, axCheck);
            p.addParameter('tList', obj.referenceTracking().tList, @isnumeric);
            p.parse(varargin{:});
            
            obj.timeList = p.Results.tList; 
            minP = p.Results.minP; maxP = p.Results.maxP; dP=(maxP-minP)/20;
            minI = p.Results.minI; maxI = p.Results.maxI; dI=(maxI-minI)/20;
            pGrid = minP:dP:maxP; iGrid = minI:dI:maxI;
            [X, Y] = meshgrid(pGrid, iGrid);
                        
            s=tf('s'); delay=exp(-s*p.Results.delay);
            cost = @(pVal, iVal) log(iecost([p.Results.dValue, pVal, iVal], obj.system.tfFunction*delay, [1 0], obj.timeList, p.Results.costfun, p.Results.weight));
            Z = arrayfun(cost, X, Y);
            
            if p.Results.isPlot                
                [C,h] = contourf(p.Results.axes, X, Y, Z); clabel(C,h); hold(p.Results.axes, 'on');
                title(p.Results.axes, sprintf('D=%5.2e', p.Results.dValue));
                plot(p.Results.axes, Kp1, Ki1,'w+');
                plot(p.Results.axes, obj.controller.p, obj.controller.i,'wo');
                
                xlabel(p.Results.axes, 'P Value'); ylabel(p.Results.axes, 'I value');
                hold(p.Results.axes, 'off');
            end
        end        
        
        %% OptimPID
        function [C, x, fval, t] = optimPID(obj, varargin)
            p=inputParser;
            typeCheck = @(x) ismember(x, {'PI', 'PID'});
            costCheck = @(x) ismember(x, {'ISE', 'ITSE', 'IAE', 'ITAE'});
            weightCheck = @(x) x>=0 && x<=1.0;
            p.addParameter('type', 'PI', typeCheck);
            p.addParameter('costfun', 'ITAE', costCheck);
            p.addParameter('weight', 1.0, weightCheck);
            p.addParameter('tList', obj.referenceTracking().tList, @isnumeric);
            p.addParameter('initialD', 1e-6, @(x) isscalar(x) && x>=0);
            p.addParameter('initialPI', [], @(x) isempty(x) || (length(x)==2 && isnumeric(x)));
            p.parse(varargin{:});
            
            try
                gain = obj.system.gain;
                G=obj.system.tfFunction/gain;
            catch
                gain = 1.0;
                G=obj.system.tfFunction;
            end
            
            obj.timeList = p.Results.tList;
            t = obj.timeList;
            % Initial parameters using stability based tuning
            switch p.Results.type
                case 'PI'
%                     [Gm,~,Wcg]=margin(G);
%                     pu=2*pi/Wcg;
%                     ku=Gm;
%                     x=ku/2.2*[1 1.2/pu];
%                     den=[1 0];

                    if isempty(p.Results.initialPI)
                        [~, tau1, tDelay] = obj.system.firstOrderApprox();
                        Kp = tau1/(obj.system.gain * (tDelay + obj.plant.tc));
                        tau_I = min(tau1, 4.0*(tDelay + obj.plant.tc));
                        x = [Kp, Kp/tau_I];
                    else
                        x = p.Results.initialPI;
                    end
                    
                    den = [1 0];
                    lb = [0 0]; hb =[inf inf];
                case 'PID'
                    [~, x0]=obj.optimPID('type', 'PI', 'costfun', p.Results.costfun, 'tList', obj.timeList);
                    x=[p.Results.initialD x0];
                    den=[1 0];
                    
                    lb = [0 0 0]; hb =[inf inf inf];
                otherwise
                    %
            end

            % redefine cost function to facilitate optimization
            cost = @(x) iecost(x, G, den, obj.timeList, p.Results.costfun, p.Results.weight);
            opt=optimset('display','off','TolX',1e-9,'TolFun',1e-9,'LargeScale','off');
            
            flag=0;
            while ~flag % if flag=0 restart optimization from current solution
                
                [x,fval,flag]=fmincon(cost,x,[], [], [], [], lb, hb, [],opt);
            end
            % the transfer function of optimal PID controller
            switch p.Results.type
                case 'PI'
                    C=EquipmentControl.FeedBackOptimization.LTIUnit.PIDController(x(1)/gain, x(2)/gain, 0.0, 'name', 'Optimal PI');
                case 'PID'
                    C=EquipmentControl.FeedBackOptimization.LTIUnit.PIDController(x(2)/gain, x(3)/gain, x(1)/gain, 'name', 'Optimal PID');
            end
        end
    end
end

function J=iecost(x,G,den,t,costfun, weight)
    t=reshape(t, [], 1);
    dt = t(2) - t(1);
    % control error of step response
    e1=1-step(feedback(G*tf(x,den),1),t);
    e2=step(feedback(G,tf(x,den)),t);
    % performance calculation
    switch costfun
        case  'ISE'
            J1=e1'*e1*dt; J2=e2'*e2*dt;
        case 'IAE'
            J1=sum(abs(e1)*dt); J2=sum(abs(e2)*dt);
        case 'ITSE'
            J1=(t.*e1*dt)'*e1; J2=(t.*e2*dt)'*e2;
        case 'ITAE'
            J1=sum(t'*abs(e1)*dt); J2=sum(t'*abs(e2)*dt);
    end
    J = weight*J1 + (1-weight)*J2;
end